#include "mpu6050.h"
#include <string.h>
#include "esp_log.h"


// I2C写操作
esp_err_t MPU_Write_Byte(uint8_t reg_addr, uint8_t data)
{
    uint8_t write_data[2] = { reg_addr, data };
    return i2c_master_write_to_device(I2C_NUM_0, MPU_ADDR, write_data, sizeof(write_data), 1000 / portTICK_PERIOD_MS);
}

// I2C读操作
esp_err_t MPU_Read_Byte(uint8_t reg_addr, uint8_t *data)
{
    // 发送寄存器地址
    esp_err_t ret = i2c_master_write_to_device(I2C_NUM_0, MPU_ADDR, &reg_addr, 1, 1000 / portTICK_PERIOD_MS);
    if (ret != ESP_OK) {
        return ret;  // 如果写寄存器地址失败，直接返回
    }

    // 读取数据
    return i2c_master_read_from_device(I2C_NUM_0, MPU_ADDR, data, 1, 1000 / portTICK_PERIOD_MS);
}


// I2C写多个字节
esp_err_t MPU_Write_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
    
    esp_err_t ret;
    uint8_t *data = malloc(len + 1);  // 为寄存器地址和数据分配空间
    if (data == NULL) return ESP_ERR_NO_MEM;

    data[0] = reg;  // 设置寄存器地址
    memcpy(data + 1, buf, len);  // 将数据填充到缓冲区

    ret = i2c_master_write_to_device(I2C_NUM_0, addr, data, len + 1, 1000 / portTICK_PERIOD_MS);
    free(data);  // 释放分配的内存
    // ESP_LOGI("MPU","%d ",ret);
    return ret;
}

// I2C读多个字节
esp_err_t MPU_Read_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
    esp_err_t ret;

    // 先写寄存器地址
    ret = i2c_master_write_to_device(I2C_NUM_0, addr, &reg, 1, 1000 / portTICK_PERIOD_MS);
    if (ret != ESP_OK) {
        return ret;
    }

    // 然后读多个字节
    ret = i2c_master_read_from_device(I2C_NUM_0, addr, buf, len, 1000 / portTICK_PERIOD_MS);
    return ret;
}


// 设置陀螺仪量程
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3);  // 设置陀螺仪量程
}

// 设置加速度计量程
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3);  // 设置加速度计量程
}


uint8_t MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data = 0;
    if (lpf >= 188) data = 1;
    else if (lpf >= 98) data = 2;
    else if (lpf >= 42) data = 3;
    else if (lpf >= 20) data = 4;
    else if (lpf >= 10) data = 5;
    else data = 6;

    return MPU_Write_Byte(MPU_CFG_REG, data);  // 设置低通滤波器
}

// 设置采样率
uint8_t MPU_Set_Rate(uint16_t rate)
{
    uint8_t data;
    if (rate > 1000) rate = 1000;
    if (rate < 4) rate = 4;
    data = 1000 / rate - 1;
    data = MPU_Write_Byte(MPU_SAMPLE_RATE_REG, data);  // 设置采样率
    return MPU_Set_LPF(rate / 2);  // 自动设置LPF
}

// 获取温度值
short MPU_Get_Temperature(void)
{
    uint8_t buf[2];
    short raw;
    float temp;
    MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
    raw = ((uint16_t)buf[0] << 8) | buf[1];
    temp = 36.53 + ((double)raw) / 340;
    return temp * 100;  // 返回温度值，单位为0.01度
}

// 获取陀螺仪数据
uint8_t MPU_Get_Gyroscope(short *gx, short *gy, short *gz)
{
    uint8_t buf[6], res;
    res = MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
    if (res == 0) {
        *gx = ((uint16_t)buf[0] << 8) | buf[1];
        *gy = ((uint16_t)buf[2] << 8) | buf[3];
        *gz = ((uint16_t)buf[4] << 8) | buf[5];
    }
    return res;
}

// 获取加速度计数据
uint8_t MPU_Get_Accelerometer(short *ax, short *ay, short *az)
{
    uint8_t buf[6], res;
    res = MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf);
    if (res == 0) {
        *ax = ((uint16_t)buf[0] << 8) | buf[1];
        *ay = ((uint16_t)buf[2] << 8) | buf[3];
        *az = ((uint16_t)buf[4] << 8) | buf[5];
    }
    return res;
}

uint8_t MPU_Init(void)
{
    uint8_t res;
    // MPU_IIC_Init();
    res=MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x80);  // 复位MPU6050
    ESP_LOGI("MPU","%d",res);
    delay_ms(100);
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x00);  // 唤醒MPU6050
    MPU_Set_Gyro_Fsr(3);                      // 设置陀螺仪量程2000dps
    MPU_Set_Accel_Fsr(0);                     // 设置加速度计量程2g
    MPU_Set_Rate(50);                         // 设置采样率50Hz
    MPU_Write_Byte(MPU_INT_EN_REG, 0x00);     // 关闭中断
    MPU_Write_Byte(MPU_USER_CTRL_REG, 0x00);  // 禁用I2C模式
    MPU_Write_Byte(MPU_FIFO_EN_REG, 0x00);    // 禁用FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG, 0x80);  // 配置INT引脚为高电平有效
    MPU_Read_Byte(MPU_DEVICE_ID_REG, &res);
    if (res == MPU_ADDR) { 
        MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x01);  // 设置时钟源
        MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0x00);  // 使能所有传感器
        MPU_Set_Rate(50);                         // 设置采样率50Hz
    } else {
        return 1;  // 初始化失败
    }
    return 0;  // 初始化成功
}